% save('data/experiment-60min.mat', 'inst_count_vec', 'rt_vec');
% load('data/experiment-60min.mat');
% load('data/experiment-40min-1servtype.mat');
classdef test_mpc2_kalman_integral < handle
    
    properties
        % model=[];
        inst_count_vec=[];
        rt_vec=[];
        m=[];
        m_inst_count_vec=[];
        m_rt_vec=[];
        samplingTime = 1;
        lqr=[];
        x0=[];
        system=[]; 
        
        kalm_x=[];
        kalm_M=[];
        sys=[];
        Q=[]; P=[]; R=[]; 
        current_state=[]; 
    end
    
    methods
        
        %  clear all
        %  clear
        %  load('data/experiment-60min.mat');
        %  open policy/test_fake_lqr.m
        %  test_fake_lqr().build_model( inst_count_vec, rt_vec)
        function obj=test_mpc2_kalman_integral(varargin)
            if nargin==1 
                obj.system=varargin{1};   
            end
        end
        
        function [m_x,d_x]=linearize(obj,x)
            m_x = mean(x);
            % d_x = x-m_x;
            d_x = x-ones(size(x,1),1)*m_x;
        end
        
        function d_x=linearize_around(obj,x,m_x)
            d_x = x-ones( size(x,1),1) * m_x ;
        end
        
        function x=delinearize_around(obj,d_x,m_x)
            x = ones( size(d_x,1),1) * m_x  + d_x;
        end
        
        function [opt_instances,opt_rt]=solve_through_qp2(obj,sys,x0,t_,m_inst_count_vec,m_rt_vec, cost_weight_r,f_inst,target_rt)
            n=3;
            m=11;
            T = t_; % horizon
%             A =  [sys.a             sys.b
%                      zeros(3,3)     eye(11)];
%             B =  [zeros(3,11)
%                      eye(11)    ]; 
            lambda = 0;
            Q = eye(n); % R = eye(m);
            r = prices_amz().get_hourly_price();
            Qhalf = sqrtm(Q); % Rhalf = sqrtm(R);
            
            % x = x0;
            varTheta0 = f_inst;
            %       cvx_precision(max(min(x)/10,1e-6))
            cvx_begin quiet
                variables rt(1,T+1) X(n,T+1) U(m,T) varTheta(m,T) varThetaSum(m,T) kappa(m,T) 
                % U +  (ones(t_,1)*m_inst_count_vec)' >= zeros(m,t_);       
                varTheta(:,2:T) ==  varTheta(:,1:T-1)  +  kappa(:,1:T-1)  
                varThetaSum == varTheta + varTheta0
                varThetaSum > zeros(m,t_);                                                          % instance quantities should be positive             
                max(ones(1,m)*varThetaSum) <= 24;             
                U == varThetaSum -  (ones(t_,1)*m_inst_count_vec)'
                X(:,2:T+1) == sys.a*X(:,1:T)+sys.b(:,1:11)* U ;    % I take noise input has mean of 0, its iid %B*U;
                rt(1, 1:T+1) ==   [1 0 0] * X(:, 1:T+1)  + ones(1,T+1) * m_rt_vec; 
                X(:,1) == x0;               
%                 varTheta(1:4,1:T) == 0;  
%                 varTheta(6:11,1:T) == 0;  
                minimize (...
                    cost_weight_r .* sum(sum_square( rt(:,2:T+1) - ones(1,T)*target_rt  ))...   % deviation cost
       ...             +sum(r'*(U+(ones(t_,1)*m_inst_count_vec)'))...                                           % resurce cost
                    +lambda*sum(X(:,T+1)));                        % final edge cost
            cvx_end
            opt_instances = U+(ones(t_,1)*m_inst_count_vec)'; % [reshape(x(t_+1:end,1),t_,m) + ones(t_,1)*m_inst_count_vec]';
            opt_rt = X(1,2:T+1) + m_rt_vec;
            subplot(3,1,1); plot(kappa');
            subplot(3,1,2); experiment().plot_instances(opt_instances');
            subplot(3,1,3); plot(opt_rt);
        end
        
        function [responsetime_,throughput_] = get_steady_state_output_for(obj,instance_quantity_to_fire,users)
            demand = 0.09;
            responsetime_ = Inf;  % if model solving fails, this is going to be the value
           % try
                opM=OpModel();
                CloudSimulate().buildtest2(opM,instance_quantity_to_fire, demand, users);
                opM.solve();
                %         util = [];
                %         for i=3:length(opM.nodes)
                %             util=[util opM.nodes(i).cpuUtilization];
                %         end
                %         util_ = mean(util);
                responsetime_ = opM.scenarios(1).responseTime;
                throughput_ = opM.scenarios(1).throughput;
%             catch err
%                 disp('error occured in system_base.getNextUtil() during model solving');
%             end
        end
        
        function  data = build_iddata(obj,inst_count_vec,rt_vec,m_inst_count_vec,m_rt_vec) 
            [  d_rt_vec]= obj.linearize_around(rt_vec , m_rt_vec);
            [ d_inst_count_vec] = obj.linearize_around(inst_count_vec,...
                m_inst_count_vec);
            
            u =  d_inst_count_vec;
            y = d_rt_vec;
            
            data=iddata(y, u , obj.samplingTime);
        end
        
        function build_model(obj, inst_count_vec, rt_vec) 
            load('data/experiment-60min.mat'); 
            obj.inst_count_vec=inst_count_vec;
            obj.rt_vec=rt_vec;
            %----------
            n_st=3;% 1 state variable
            n_in= size(inst_count_vec,2);  %11 inputs
            n_out= 1; %size(rt_vec,2); % 1 output
            n_env=1; % environmental disturbances
           % A=ones(n_st,n_st); 
            A=[1 0 0
                  zeros(2,1) ones(2,2)];
            B=ones(n_st,n_in);
            K = [1;1;1]; % ones(n_st,n_env);
            C = [1 0 0];  % eye(n_out,n_st);
            D=zeros(n_out,n_in);
            
            
%             [m_rt_vec,              d_rt_vec]= obj.linearize(rt_vec);
%             [m_inst_count_vec,  d_inst_count_vec] = obj.linearize(inst_count_vec);
      
            % set the instances so around the desired point, allocatet your
            % self , 2 sec desire
            m_inst_count_vec = [2 1 3 4 1 1 1 1 1 0 0]; 
                    %  3     4     1     1     1     0     0     2     1     0     0
            users = 800; 
            m_rt_vec = obj.get_steady_state_output_for( m_inst_count_vec , users); 
           data = obj.build_iddata(inst_count_vec,rt_vec,m_inst_count_vec,m_rt_vec); 
            m=idss(A,B,C,D,K,'Ts', obj.samplingTime,'DisturbanceModel','Estimate','InitialState','Estimate');  % None            
            m.Cs = C;
    %        m.Ks = [NaN;NaN;NaN];
            m1=init(m);
            mm=pem(data,m1)
            
            obj.m=mm;
            obj.m_inst_count_vec=m_inst_count_vec;
            obj.m_rt_vec = m_rt_vec;           
            
            obj.sys = ss(obj.m); % ss(obj.m , 'm'); 
            kalm_sys = obj.sys; 
            obj.Q = 1; obj.R = 0;
      %      [kalmf,L,P,kalm_M] = kalman(kalm_sys,Q,R);
            obj.P=kalm_sys.b*obj.Q*kalm_sys.b';         % Initial error covariance
    %        obj.kalm_M = kalm_M;
    
           A=obj.sys.a;
           B=obj.sys.b;
           C=obj.sys.c;
           D=obj.sys.d;
           x0=obj.m.x0; 
          save('data/test_kalman.mat', 'x0', 'A','B','C','D','m_inst_count_vec', 'm_rt_vec');
                     
        end
           
        function  [instance_quantity_to_fire,instance_quantity_to_destroy]=apply_heuristic(obj,opt_instances,opt_rt,f_inst) 
            control_affect_window=2; 
            instance_quantity_to_destroy=zeros(1,11); 
            % see how much instances we need more than allocated in the
            % control_affect_window. then take the mean over the window 
            % and allocate the extra         
            instance_quantity_to_fire = opt_instances(:,1:control_affect_window)...
                                            - f_inst(:,1:control_affect_window);
   %         instance_quantity_to_fire=abs(instance_quantity_to_fire);
             instance_quantity_to_fire = mean(instance_quantity_to_fire,2);  
             instance_quantity_to_fire(instance_quantity_to_fire<0)=0;
     
            instance_quantity_to_fire = round(instance_quantity_to_fire)'; 
            % see system current number of inst.....
        end
        
        %    test_lqr_policy().run_test
        function [instance_quantity_to_fire,instance_quantity_to_destroy]=get_choice_at_time(obj,t)
           % update the model state based on system response
            % compute unknown states based on the most recent rt if there
            % is such rt, other wise use the initial state of the model 
            
            if (t<12)
                target_rt = 12;
            else
                target_rt = 5;
            end
            
            f_inst = obj.system.future_inst()';
            % grab info on already allocated instances 
             t_=60;
            if size(f_inst,2)<t_
                f_inst=[f_inst zeros(11,t_-size(f_inst,2))];           
            end        
            
            leave_interval = 20; 
            % leave it for a while          
            if mod(t,leave_interval)~=1
                instance_quantity_to_fire=zeros(1,11);   
                instance_quantity_to_destroy=zeros(1,11);
                return
            else
                if (t==1)
                    obj.current_state = obj.m.x0; 
                else    % t=21, 41     
                     past_rt=obj.system.past_util();         
                     past_acc_inst=obj.system.past_inst();    
                     data = obj.build_iddata(past_acc_inst, past_rt, obj.m_inst_count_vec, obj.m_rt_vec) ; 
                     [e,current_state_1] = pe(obj.m,data,'e(stimate)');       
                       A=obj.sys.a;  B=obj.sys.b;
                       C=obj.sys.c;  D=obj.sys.d;
                      % x0=obj.m.x0; 
                     obj.current_state =  test_kalman(A,B,C,D, past_acc_inst(leave_interval,1)-ones(leave_interval,1)*obj.m_inst_count_vec, past_rt-obj.m_rt_vec, obj.current_state, leave_interval);  
                %     test_kalman(obj.m.x0,past_acc_inst,past_rt)                 
                 %   current_state=obj.current_state;  save('data/test_kalman.mat',  'past_acc_inst','past_rt','current_state', '-append');
                     
                end  
                cost_weight_r=4;
            %  [opt_instances,opt_rt] = obj.solve_through_qp(sys,obj.m.x0,t_, obj.m_inst_count_vec, obj.m_rt_vec, cost_weight_r,f_inst,target_rt);       
                [opt_instances,opt_rt] = obj.solve_through_qp2(obj.sys, obj.current_state ,t_, obj.m_inst_count_vec, obj.m_rt_vec, cost_weight_r,f_inst,target_rt);       
                [instance_quantity_to_fire,instance_quantity_to_destroy] = obj.apply_heuristic(opt_instances,opt_rt,f_inst);         
            end
            
        end
        
         % ------------------------------------------ test -----------------------------------
            %             load('data/experiment-60min.mat');
            %            load('data/experiment-40min-1servtype.mat');
            % %            noise=randn(60,2)*0.001;
            %             x_sys = mm.X0; % system state  for the first time
            %             rt_acc=[];
            %             u_opt_acc=[];
            %             for i=1:20
            %                 %------ system update ------
            %                 x_sys =sys.a*x_sys + sys.b* (opt_instances(:,i)-m_inst_count_vec') ;
            %                 rt_acc=[rt_acc x_sys+m_rt_vec];
            %                 % u_opt_acc=[u_opt_acc ; u_opt'];
            %             end
            %             rt_acc
            
            %             reference = [ones(1,20)*8 ones(1,20)*6
            %             ones(1,20)*7];
            %             d_reference = obj.linearize_around(reference , m_rt_vec);  %  2  seconds desired
            %
            %
            %             t= 0:1:60-1;
            %
            %          [y,t,x] =    lsim(sys_all, [d_reference ; input] ,t);
            %          figure
            %          u=UtilityLib();
            %          [0.4,0.4,0.4]
            %          % plot(reference, strcat(u.lnstyle{2}),...
            %          plot(d_reference, strcat(u.lnstyle{2}),...
            %                 'LineWidth',2,...
            %                 'MarkerSize',10);
            %             hold on;
            %         % plot(obj.delinearize_around(y(:,1) , m_rt_vec), strcat(u.lnstyle{3},u.color{7}),...
            %          plot(y(:,1) , strcat(u.lnstyle{3},u.color{7}),...
            %                 'LineWidth',2,...
            %                 'MarkerSize',10);
            %          legend('Reference','System')
            %          title('System/Reference response time');
            %
            %          figure;
            %          alloc_instances =  obj.delinearize_around(y(:,2:12) , m_inst_count_vec);
            %          % plot_instances(alloc_instances); legend(prices_amz().get_api_names');
            %          plot_instances(y(:,2:12)); legend(prices_amz().get_api_names');
            %          legend(prices_amz().get_api_names')
            %          title('Number of Instances');
            %
            
    end
    
end

